function UDP_Server(block)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%
	setup(block);

function setup(block)
	block.NumDialogPrms = 2;

	block.NumInputPorts = 0;
	block.NumOutputPorts = 2;

	block.SetPreCompInpPortInfoToDynamic;
  block.SetPreCompOutPortInfoToDynamic;

  block.OutputPort(1).Complexity   = 'Real';
  block.OutputPort(1).DataTypeId   = 4;
  block.OutputPort(1).SamplingMode = 'Sample';
  block.OutputPort(1).Dimensions   = 16;

  block.OutputPort(2).Complexity   = 'Real';
  block.OutputPort(2).DataTypeId   = 7;
  block.OutputPort(2).SamplingMode = 'Sample';
  block.OutputPort(2).Dimensions   = 1;

	%block.SampleTimes = [SampleTime 0];
	block.SampleTimes = [10/1000 0]; % how do I get varables from the workspace?

	block.RegBlockMethod('PostPropagationSetup',    @DoPostPropSetup);
	block.RegBlockMethod('Start',                   @Start);  
	block.RegBlockMethod('Outputs',                 @Outputs);
	block.RegBlockMethod('Terminate',               @Terminate);

function DoPostPropSetup(block)
	block.NumDworks = 1;

	block.Dwork(1).Name            = 'socket';
	block.Dwork(1).Dimensions      = 1;
	block.Dwork(1).DatatypeID      = 0;      % double
	block.Dwork(1).Complexity      = 'Real'; % real
	block.Dwork(1).UsedAsDiscState = true;

function Start(block)
	port = block.DialogPrm(2).data;

	udp=pnet('udpsocket',port);
	pnet(udp,'setreadtimeout',1/1000);
	block.Dwork(1).Data = udp;

function Outputs(block)
	udp = block.Dwork(1).Data;

	servo_angles = zeros(16,1,'int16');
	servo_pulses = zeros(16,1,'uint16');
	time = zeros(1,1,'uint32');
	conf = zeros(1,1,'uint32');

	servo_type = 0;
	conf_type  = 1;

	try,
		len = pnet(udp,'readpacket','noblock');
    if len <= 0
			return
		end
    type = pnet(udp,'read',1,'uint8');

		switch type
			case {servo_type}
				for i = 1:16
					servo_pulses(i,1)=pnet(udp,'read',1,'uint16');
					servo_angles(i,1)=pnet(udp,'read',1,'int16');
				end
				%servo_angles = swapbytes(servo_angles);
				%servo_pulses = swapbytes(servo_pulses);
				block.OutputPort(1).Data = servo_angles;
			case {conf_type}
				data = pnet(udp,'read',2,'uint32');
				%data = swapbytes(data);
				time=data(1);
				conf=data(2);
				block.OutputPort(2).Data = time;
		end
	end

function Terminate(block)
	udp = block.Dwork(1).Data;
	pnet(udp,'close');
